# -*- coding: utf-8 -*-
# @Author: obsidian
# @Date:   2016-04-02 20:06:14
# @Last Modified by:   obsidian
# @Last Modified time: 2016-06-04 10:04:16
import numpy as np
import matplotlib.pyplot as plt

front_x = 0
front_y = 1
rear_x = 2
rear_y = 3
angle = 4
speed = 5
diserr = 6
angerr = 7
steerang = 8
index = 9
led_off = 10
encoder1 = 11
target_angle = 12

l = eval('[' + open('/media/obsidian/E867-674A/path.txt','r').read(-1).replace('}',']').replace('{','[') + ']')
#l = eval('[' + open('pathchongchu.txt','r').read(-1).replace('}',']').replace('{','[') + ']')
#l = eval('[' + open('path.txt','r').read(-1).replace('}',']').replace('{','[') + ']')

a = np.array(l)
plt.plot(a[:,0] / 10.0,a[:,1] / 10.0,'ro')
plt.plot(a[:,rear_x] / 10.0,a[:,rear_y] / 10.0,'yo')
# plt.show()
f = open('../../../car8/code/control/GPSpath/pointpath.c')
#f = open('../control/GPSpath/pointpath.c')

lis = eval(f.read(-1).split('=')[1].replace('{','[').replace('}',']').replace('\r','').replace('\n','').replace(';',''))
x = []
y = []
for i in range(len(lis)):
    x.append(lis[i][0][0])
    y.append(lis[i][0][1])
plt.plot(x,y,'b')
plt.plot(a[:,index],a[:,diserr]/10.0,'bo')
plt.plot(a[:,index],a[:,angerr]/10.0,'ro')
# plt.plot(a[:,index],a[:,angle]/10.0,'yo')
plt.plot(a[:,index],a[:,steerang]/10.0,'yo')
plt.plot(a[:,index],a[:,speed]/10.0,'g')
plt.plot(a[:,index],a[:,led_off],'go')
# plt.plot(a[:,index],a[:,rear_y]/10.0,'g')
# #x = [i for i in range(len(a))]
plt.plot(a[:,index],a[:,angle] / 10.0,'b+')
plt.plot(a[:,index],a[:,target_angle],'r')
# plt.plot(a[:,index],a[:,encoder2]/10.0,'ro')
plt.plot([687,-19,1041,334],[7128,7128,6633,6633],'r+')
plt.show()
